package demo.axitdn.physic;

import java.util.ArrayList;
import java.util.List;

public class RK4V {
	private Force derivative;
    private Unit decac=new Unit();
    public RK4V(Force derivative) {
        this.derivative = derivative;
    }
        
    private Derivative evaluate(State state, double t) {
        Derivative output = new Derivative();
        Vector3f dr=null;
        State s=state.copy();
        dr=derivative.ff(s, t);
        output.dP=(Vector3f) s.velocity.clone();
        output.dV=(Vector3f) dr;
        return output;
    }

    private Derivative evaluate(State init, double t, float dt, Derivative d) {
        State state = new State();
        state.position.x=init.position.x+d.dP.x*dt;
        state.position.y=init.position.y+d.dP.y*dt;
        state.position.z=init.position.z+d.dP.z*dt;
        
        state.velocity.x=init.velocity.x+d.dV.x*dt;
        state.velocity.y=init.velocity.y+d.dV.y*dt;
        state.velocity.z=init.velocity.z+d.dV.z*dt;
        
        Derivative output = new Derivative();
        Vector3f dr=null;
        State s=state.copy();
        dr=derivative.ff(s, t);
        output.dP=(Vector3f) s.velocity.clone();
        output.dV=(Vector3f) dr;
        return output;
    }

    public void integrate(State state, double t, float dt) {
        Derivative a = evaluate(state, t);
        Derivative b = evaluate(state, t, dt * 0.5f, a);
        Derivative c = evaluate(state, t, dt * 0.5f, b);
        Derivative d = evaluate(state, t, dt, c);
        
        float dxdt = 1.0f / 6.0f * (a.dP.x + 2.0f * (b.dP.x + c.dP.x) + d.dP.x);
        float dydt = 1.0f / 6.0f * (a.dP.y + 2.0f * (b.dP.y + c.dP.y) + d.dP.y);
        float dzdt = 1.0f / 6.0f * (a.dP.z + 2.0f * (b.dP.z + c.dP.z) + d.dP.z);
        
        float dvxdt = 1.0f / 6.0f * (a.dV.x + 2.0f * (b.dV.x + c.dV.x) + d.dV.x);
        float dvydt = 1.0f / 6.0f * (a.dV.y + 2.0f * (b.dV.y + c.dV.y) + d.dV.y);
        float dvzdt = 1.0f / 6.0f * (a.dV.z + 2.0f * (b.dV.z + c.dV.z) + d.dV.z);
        
        
        state.position.x+=dxdt*dt;
        state.position.y+=dydt*dt;
        state.position.z+=dzdt*dt;
        
        state.velocity.x+=dvxdt*dt;
        state.velocity.y+=dvydt*dt;
        state.velocity.z+=dvzdt*dt;
        
    }

    public State interpolate(State previous, State current, float alpha) {
        State state = new State();
        previous.position.interpolate(current.position, alpha);
        state.position=previous.position;
        
        previous.velocity.interpolate(current.velocity, alpha);
        state.velocity=previous.velocity;
        
        return state;
    }

	public Force getForce() {
		// TODO Auto-generated method stub
		return derivative;
	}
}
